#include "cubeCommands.h"
#include "cubeFunctions.h"

char * commandVector[MAX_COMMANDS] = { "BLINK" };
int (*functionVector[MAX_COMMANDS])(void) = { blinkLed };

bool commandsInitialized = false; 


int getExCommand(int fd)
{
	char cmdBuffer[CMD_SIZE];
	cmdBuffer[CMD_SIZE-1] = '\0';
	bool tryAgain = true; 
	// Get characters until it's filled
	int index = 0; 
	for(int timeouts=0; tryAgain; timeouts++)
	{
		if(timeouts > 9)	// Timed out
			return -1;
		while(serialDataAvail(fd) > 0 && tryAgain)
		{
			char c = serialGetchar(fd);
			cmdBuffer[index] = c;
			index++; 
			if(c = '!' || index >= (CMD_SIZE - 1)){
				tryAgain = false; 
				cmdBuffer[index-1] = '\0';
				cmdBuffer[CMD_SIZE-1] = '\0'; 
				//std::cout<< cmdBuffer << std::endl; 
			}
			//std::cout<< cmdBuffer << std::endl; 
		}
		//std::cout<< cmdBuffer << std::endl; 
		sleep(1);
	} 
	
	int i = getCmdNum(cmdBuffer);
	if(i >= 0){
		std::cout << "Calling function:\t" << i << std::endl; 	
		functionVector[i](); 	
	}
	else{
		std::cout << "Error Finding CMD" << std::endl; 
		return -1;
	}

	return 0; 

}


int getCmdNum(char * cmdStr)
{
	if(cmdStr == NULL)
		return -1;
	else{
		//for(int i=0; i<CMD_SIZE; i++)
		//{
		//	std::cout<< cmdStr[i] << std::endl; 
		//}
		for(int i=0; i<MAX_COMMANDS; i++)
		{
			std::cout<< commandVector[i] << std::endl; 
			if(strcmp(cmdStr, commandVector[i]) == 0)
				return i;
		}
		
		return -1;	// Not found
	}
}
